#include "GPUPlanner.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/util/Time.h"
#include <limits>
#include <iostream>
#include <fstream>
#include "ompl/base/SpaceInformation.h"
#include "ompl/base/spaces/RealVectorBounds.h"
#include "ompl/base/spaces/SE2StateSpace.h"
#include <boost/math/constants/constants.hpp>

using namespace ompl;

GPUPlanner::GPUPlanner(const base::SpaceInformationPtr &si, const app::GeometrySpecification* geometrySpecification) 
	: base::Planner(si, "GPUPlanner"), geometrySpecification_(geometrySpecification)
{
	specs_.approximateSolutions = true;
	specs_.directed = true;
	specs_.multithreaded = false;

	maxDistance_ = 0.0;

	Planner::declareParam<double>("range", this, &GPUPlanner::setRange, &GPUPlanner::getRange, "0.:1.:10000.");
}

GPUPlanner::~GPUPlanner(void)
{
	freeMemory();
}

void GPUPlanner::clear(void)
{
	Planner::clear();
	GPUPlanner::freeMemory();
}

void GPUPlanner::setup(void)
{
	Planner::setup();
	tools::SelfConfig sc(si_, getName());
	sc.configurePlannerRange(maxDistance_);

	getSpaceBounds();
}

void GPUPlanner::freeMemory(void)
{
}

void GPUPlanner::getPlannerData(base::PlannerData &data) const
{
	Planner::getPlannerData(data);
}

void GPUPlanner::getSpaceBounds()
{
	const double SO3_SCALE = 1.0;
	unsigned int uiDimension = si_->getStateDimension();

	vBoundsMin_.resize(uiDimension);
	vBoundsMax_.resize(uiDimension);

	std::vector<base::StateSpacePtr> vSubSpaces = si_->getStateSpace()->as<base::CompoundStateSpace>()->getSubspaces();
	int uiDimIndex = 0;
	for (unsigned int i = 0; i < vSubSpaces.size(); ++i)
	{
		int uiDim = vSubSpaces[i]->getDimension();
		switch(vSubSpaces[i]->getType())
		{
		case base::STATE_SPACE_REAL_VECTOR:
			{
				const base::RealVectorBounds& bounds = vSubSpaces[i]->as<base::RealVectorStateSpace>()->getBounds();
				for (int j = 0; j < uiDim; ++j)
				{
					vBoundsMin_[uiDimIndex + j] = bounds.low[j];
					vBoundsMax_[uiDimIndex + j] = bounds.high[j];
				}
			}
			break;

		case base::STATE_SPACE_SO2:
			{
				vBoundsMin_[uiDimIndex] = -boost::math::constants::pi<double>();
				vBoundsMax_[uiDimIndex] = boost::math::constants::pi<double>();
			}
			break;

		case base::STATE_SPACE_SO3:
			{
				for (int j = 0; j < uiDim; ++j)
				{
					vBoundsMin_[uiDimIndex + j] = -1 * SO3_SCALE;
					vBoundsMax_[uiDimIndex + j] = 1 * SO3_SCALE;
				}
			}
			break;

		default:
			OMPL_INFORM("Unsupported state space.");
			break;
		}
		uiDimIndex += uiDim;
	}
}